#ifndef __KALMAN_FILTER_H
#define __KALMAN_FILTER_H

#include <stdint.h>

// 简化卡尔曼滤波器结构体（2维状态：角度 + 陀螺仪零偏）
typedef struct {
    float angle;         // 估计角度（俯仰角/横滚角）
    float bias;          // 陀螺仪零偏估计
    float P[2][2];       // 简化协方差矩阵（仅保留主对角线附近元素）
    float Q_angle;       // 过程噪声-角度（模型不确定性）
    float Q_bias;        // 过程噪声-零偏（零偏漂移率）
    float R_measure;     // 测量噪声（加速度计噪声）
} KalmanFilter;

// 滤波器初始化（设置初始参数）
void Kalman_Init(KalmanFilter *kf, float Q_angle, float Q_bias, float R_measure);

// 卡尔曼滤波器更新（传入陀螺仪角速度、加速度计角度、时间差dt）
void Kalman_Update(KalmanFilter *kf, float gyro_rate, float accel_angle, float dt);

// 获取当前估计角度（直接访问结构体成员）
#define Kalman_GetAngle(kf) ((kf)->angle)

#endif